#include <wsnos.h>

#ifdef OSEL_STATIC_MEMMORY

#if CFG_MEM_ALIGNMENT == 4
#define HEAP_BYTE_ALIGNMENT_MASK	((osel_uint32_t)0x0003)
#endif

#if CFG_MEM_ALIGNMENT == 2
#define HEAP_BYTE_ALIGNMENT_MASK	((osel_uint32_t)0x0001)
#endif

#if CFG_MEM_ALIGNMENT == 1 
#define HEAP_BYTE_ALIGNMENT_MASK	((osel_uint32_t)0x0000)
#endif

#ifndef HEAP_BYTE_ALIGNMENT_MASK
#error "Invalid CFG_MEM_ALIGNMENT definition"
#endif

static struct
{
	osel_uint16_t padding;
	osel_uint8_t store[CFG_HEAP_SIZE];
} heap;

static osel_uint32_t next_free_byte = (osel_uint32_t)0;

void osel_mem_init(void)
{
	next_free_byte = (osel_uint32_t)0;
}

void *osel_mem_alloc(osel_uint16_t size)
{
	void *mem = NULL; 

#if CFG_MEM_ALIGNMENT != 1
	if( size & HEAP_BYTE_ALIGNMENT_MASK )
	{
		size += (CFG_MEM_ALIGNMENT - (size & HEAP_BYTE_ALIGNMENT_MASK));
	}
#endif

    OSEL_INT_LOCK();
	{
		
		if ((( next_free_byte + size) <= CFG_HEAP_SIZE) &&
			(( next_free_byte + size) > next_free_byte))
		{
			mem = &(heap.store[next_free_byte]);
			next_free_byte += size;
		}
	}
    OSEL_INT_UNLOCK();

	return mem;
}

void osel_mem_free(void *mem)
{
	(void)mem;
}

void osel_memset(void *dst, osel_uint8_t val, osel_uint16_t len)
{
	OSEL_ASSERT(dst != OSEL_NULL);
	osel_uint8_t *p = (osel_uint8_t *)dst;
    
	while(len--)
	{
		*p++ = val;
	}
}

void osel_memcpy(void * dst, const void *src, osel_uint16_t len)
{
	OSEL_ASSERT(dst != OSEL_NULL);
	OSEL_ASSERT(src != OSEL_NULL);
	osel_uint8_t *tmp = (osel_uint8_t *)dst;
	osel_uint8_t *s = (osel_uint8_t *)src;

	while (len--)
	{
		*tmp++ = *s++;
	}
}


#endif

